from dronekit import connect,VehicleMode
import time
from enum import Enum
from pymavlink import mavutil, dialects  # Needed for command message definitions
from pytictoc import TicToc

# RESET_SEQ = "\033[0m"
# COLOR_SEQ = "\033[1;%dm"
# BOLD_SEQ = "\033[1m"
# colors_On_Red='\033[41m' 
# colors_Normal='\e[0m' 
# import logging
# from my_log import ColoredLogger
# logging.setLoggerClass(ColoredLogger)
# color_log = logging.getLogger(__name__)
# # color_log.setLevel(logging.DEBUG)

# color_log.debug("test")
# color_log.info("test")
# color_log.warning("test")
# color_log.error("test")
# color_log.critical("test")
class Control_Mode(Enum):
    MANUAL = 1
    OPTICAL_FLOW = 2
    PROGRAM_MODE = 3
class apm_quad_x:
    def __init__(self,device='/dev/ttyACM0'):
        self.flight_height = 1.2
        self.x_vel_set=0
        self.y_vel_set=0
        self.z_vel_out = 0
        self.yaw_out = 0
        self.control_activate=False

        t = TicToc()
        t.tic() #Start timer
        # print("Address of self = ",id(self))
        self.vehicle= connect(device, wait_ready=True,baud=921600,rate=20)
        print(("Autopilot Firmware version: %s" % self.vehicle.version))
        # print("Channel values from RC Tx:", self.vehicle.channels)

        print("vehicle connect,wait mesg.....")
        # time.sleep(12)
        self.flight_mode=0
        self.control_mode=Control_Mode.MANUAL
        self.last_control_mode=Control_Mode.MANUAL

        self.vehicle._hao_last_update_time=0

        @self.vehicle.on_message('RC_CHANNELS')
        def chin_listener(self, name, message):
            # about 4hz
            self._hao_last_update_time = time.time()
            # print("Address of self = ",id(self))
            # print '%s attribute is: %s' % (name, message)
            self.rc_pitch=message.chan2_raw    # copter (throttle)
            self.rc_roll=message.chan1_raw     # copter (angle)
            self.rc_throttle=message.chan3_raw # copter (throttle)
            self.rc_yaw=message.chan4_raw # copter (yaw)
            self.rc_mod=message.chan6_raw      # fly, stop, shift left, right, switch SD
            self.rc_flag_rec=message.chan7_raw # drive - record mode, switch SG
            # print('channels:',self.pitch,self.roll,self.mod,self.flag_rec)
            # print(message)
            # print(type(message))
        while time.time()-self.vehicle._hao_last_update_time>0.25:
            time.sleep(0.1)
        t.toc("init cost")

        self.last_battery_time = time.time()
        self.last_rangefinder_time = time.time()
        print("Initial Mode %s"%self.vehicle.mode.name)
        print("Set Mode LOITER")
        self.vehicle.mode = VehicleMode("LOITER")
        time.sleep(0.1)
        self.last_flight_mode=self.vehicle.mode.name
        self.last_flight_mode_save= self.vehicle.mode.name
        self.move_task_times = 0
        self.link_alive = True
        self.offboard_safety = True
        self.last_offboard_safety = False
        self.is_takeoff_running = False
        self.is_takeoff = False
        self.is_land_running = False
    @property
    def rc_pitch(self):
        return self.vehicle.rc_pitch
    @property
    def rc_roll(self):
        return self.vehicle.rc_roll
    @property
    def rc_yaw(self):
        return self.vehicle.rc_yaw
    @property
    def rc_throttle(self):
        return self.vehicle.rc_throttle
    @property
    def rc_mod(self):
        return self.vehicle.rc_mod
    @property
    def rc_safety(self):
        return self.vehicle.rc_flag_rec

    @property
    def height_laser(self):
        return self.vehicle.rangefinder.distance

    def rc_task(self):
        # print(self.rc_mod)
        if(self.rc_safety<1500):
            self.offboard_safety = True
            # print("offboard_safety on")
        else:
            self.offboard_safety = False
            # print("offboard_safety off")

        if(self.last_offboard_safety != self.offboard_safety ):
            print("offboard_safety",self.offboard_safety)
        self.last_offboard_safety = self.offboard_safety
        
        if(self.rc_mod<1500):
            self.control_mode=Control_Mode.OPTICAL_FLOW
        else:
            self.control_mode=Control_Mode.PROGRAM_MODE

        # if(self.last_control_mode != self.control_mode ):
        #     print("Control Mode SET",self.control_mode)
        # self.last_control_mode = self.control_mode
    def battery_task(self):
        if(time.time()-self.last_battery_time > 3):
            voltage = self.vehicle.battery.voltage
            percentage = (voltage-3.7*4)/((4.2-3.7)*4)
            print("battery{ voltage:%f  percentage:%f}" % (voltage,percentage))    # settable

            self.last_battery_time = time.time()
    def distance_task(self):
        if(time.time()-self.last_rangefinder_time > 1):
            # print("[property] Rangefinder distance: %s" % self.vehicle.rangefinder.distance)
            # print("[property] Attitude: %s" % self.vehicle.attitude)
            # print("[property] Local Location: %s" % self.vehicle.location.local_frame)   #NED
            # print("[property] Velocity: %s" % self.vehicle.velocity)

            self.last_rangefinder_time = time.time()
        



    def flight_mode_task(self):
        self.flight_mode = self.vehicle.mode.name

        if(self.control_mode == Control_Mode.PROGRAM_MODE and self.last_control_mode != Control_Mode.PROGRAM_MODE):
            self.vehicle.mode = VehicleMode("GUIDED")
            self.last_control_mode = self.control_mode 
            print("Mode: GUIDED")    # settable
            print("SAVE Mode: %s" % self.flight_mode)    # settable
            self.last_flight_mode_save = self.flight_mode
        elif(self.control_mode == Control_Mode.OPTICAL_FLOW and self.control_mode != self.last_control_mode):
            
            self.vehicle.mode = VehicleMode(self.last_flight_mode_save)
            self.last_control_mode = self.control_mode 
            print("[Mode] %s" % self.last_flight_mode_save)    # settable
            print("[WARN] 若LAND完成前就退出GUID模式,则需重新启动才可再次使用")
            # print("Mode: %s" % self.flight_mode)    # settab
            
    def move_task(self):
        # 这里不能用 time.sleep 不要阻塞
        if(self.vehicle.mode.name == "GUIDED" and self.offboard_safety == False):
            self.move_task_times = self.move_task_times + 1
            if(self.move_task_times == 50):
                self.vehicle.armed  = True
            elif(self.move_task_times == 100):
                print("[TASK] Taking off!")
                self.vehicle.simple_takeoff(1)
            elif(self.move_task_times == 150):
                self.control_activate = True
                self.flight_height = 1.2
                self.x_vel_set=0
                print("[TASK] vehicle x axis move %f m/s vehicle y axis move %f m/s"%(self.x_vel_set,self.y_vel_set))
            elif(self.move_task_times == 200):
                self.control_activate = True
                self.flight_height = 1.2
                self.x_vel_set=0.2
                print("[TASK] vehicle x axis move %f m/s vehicle y axis move %f m/s"%(self.x_vel_set,self.y_vel_set))
            elif(self.move_task_times == 250):
                self.control_activate = True
                self.flight_height = 1.2
                self.x_vel_set=0
                self.y_vel_set= 0
                print("[TASK] vehicle x axis move %f m/s vehicle y axis move %f m/s"%(self.x_vel_set,self.y_vel_set))
            elif(self.move_task_times == 300):
                self.control_activate = True
                self.flight_height = 1.2
                self.x_vel_set=0
                self.y_vel_set=0
                print("[TASK] vehicle x axis move %f m/s vehicle y axis move %f m/s"%(self.x_vel_set,self.y_vel_set))
            elif(self.move_task_times == 350):
                self.control_activate = True
                self.flight_height = 1.2
                self.x_vel_set=0
                self.y_vel_set=0
                self.yaw_out = 10
                # self.drone_set_hover()
                # self.drone_rot_left(15)
                print("[TASK] vehicle x axis move %f m/s vehicle y axis move %f m/s"%(self.x_vel_set,self.y_vel_set))
            elif(self.move_task_times == 400):
                self.control_activate = True
                self.flight_height = 1.2
                self.x_vel_set=0
                self.y_vel_set=0
                self.yaw_out = 20
                # self.drone_rot_right()
                print("[TASK] vehicle x axis move %f m/s vehicle y axis move %f m/s"%(self.x_vel_set,self.y_vel_set))
            
        

            # 独立 ,确保正常退出
            if(self.move_task_times == 500):
                self.control_activate = False
                self.link_alive = False
                print("mission complete")
        else:
            self.move_task_times = 0

    def drone_rot_left(self,heading=45):
        is_relative = 1
        # create the CONDITION_YAW command using command_long_encode()
        msg = self.vehicle.message_factory.command_long_encode(
            0, 0,  # target system, target component
            mavutil.mavlink.MAV_CMD_CONDITION_YAW,  # commandqf
            0,  # confirmation
            heading,  # param 1, yaw in degrees
            0,  # param 2, yaw speed deg/s
            -1,  # param 3, direction -1 ccw, 1 cw
            is_relative,  # param 4, relative offset 1, absolute angle 0
            0, 0, 0)  # param 5 ~ 7 not used
        # send command to vehicle
        self.vehicle.send_mavlink(msg)
        print("vehicle left rotate %f degrees"%heading)
    def drone_rot_right(self,heading=45):
        is_relative = 1
        msg = self.vehicle.message_factory.command_long_encode(
            0, 0,  # target system, target component
            mavutil.mavlink.MAV_CMD_CONDITION_YAW,  # command
            0,  # confirmation
            heading,  # param 1, yaw in degrees
            0,  # param 2, yaw speed deg/s
            1,  # param 3, direction -1 ccw, 1 cw
            is_relative,  # param 4, relative offset 1, absolute angle 0
            0, 0, 0)  # param 5 ~ 7 not used
        # send command to vehicle
        self.vehicle.send_mavlink(msg)
        print("vehicle right rotate %f degrees"%heading)

    def drone_height_control(self):
        MAX_SPEED = 0.30 #m/s
        z_CONTROL_P = 1.5
        z_vel = - z_CONTROL_P*(self.flight_height - self.height_laser)
        if(z_vel > MAX_SPEED):
            z_vel = MAX_SPEED
        elif(z_vel < -MAX_SPEED):
            z_vel = -MAX_SPEED

        if(self.vehicle.mode.name == "GUIDED" and self.offboard_safety == False):
            self.z_vel_out = z_vel
        else:
            self.z_vel_out = 0
        # print(self.z_vel_out)
    def drone_translation_out_task(self):
        # z + up - down
        # x + forward - back
        # y + right - left
        if(self.control_activate == True):       
            msg = self.vehicle.message_factory.set_position_target_local_ned_encode(
                0,  # time_boot_ms (not used)
                0, 0,  # target system, target component
                mavutil.mavlink.MAV_FRAME_BODY_NED,  # frame
                0b0000111111000111,  # type_mask (only speeds enabled)
                0, 0, 0,  # x, y, z positions (not used)
                self.x_vel_set, self.y_vel_set, self.z_vel_out,  # x, y, z velocity in m/s
                0, 0, 0,  # x, y, z acceleration (not supported yet, ignored in GCS_Mavlink)
                self.yaw_out, 0)  # yaw, yaw_rate (not supported yet, ignored in GCS_Mavlink)
            self.vehicle.send_mavlink(msg)
    def drone_set_hover(self):
        msg = self.vehicle.message_factory.set_position_target_local_ned_encode(
            0,  # time_boot_ms (not used)
            0, 0,  # target system, target component
            mavutil.mavlink.MAV_FRAME_BODY_NED,  # frame
            0b0000111111000111,  # type_mask (only speeds enabled)
            0, 0, 0,  # x, y, z positions (not used)
            0, 0, 0,  # x, y, z velocity in m/s
            0, 0, 0,  # x, y, z acceleration (not supported yet, ignored in GCS_Mavlink)
            0, 0)  # yaw, yaw_rate (not supported yet, ignored in GCS_Mavlink)
        self.vehicle.send_mavlink(msg)





    def close_mission(self):
        self.vehicle.mode = VehicleMode("LAND")
        time.sleep(2)
        while self.vehicle.armed:
            print(" Waiting for disarm")
            time.sleep(1)
            self.vehicle.armed   = False

        print("ready to close")
        self.vehicle.close()

    def task_manager(self):
        self.rc_task()
        self.flight_mode_task()

        self.battery_task()

        self.drone_height_control()

        # self.move_task()
        self.distance_task()
        self.drone_translation_out_task()

    
    def takeoff(self):
        self.vehicle.armed  = True
        if(self.vehicle.armed):
            if(self.is_takeoff_running is False and self.is_takeoff is False):
                self.vehicle.simple_takeoff(1.2)
                self.is_takeoff_running = True
                print("[ECHO] drone start takeoff")
            if(self.height_laser > 0.9):
                print("[ECHO] drone takeoff")
                self.control_activate = True
                print("[ECHO] drone control : activate")
                self.is_takeoff = True
                self.is_takeoff_running = False
                self.flight_height = 1.2
                self.x_vel_set=0
                self.y_vel_set=0   

        else:
            print(" Waiting for arm")
    def land(self):
        if(self.vehicle.mode.name != "LAND"):
            self.vehicle.mode = VehicleMode("LAND")
            print("[ECHO] drone start land")
            self.is_land_running = True
        if self.height_laser < 0.03 and self.is_land_running:
            self.is_land_running = False
            self.is_takeoff = False
            print("[ECHO] drone land")
        else:
            print(" Waiting for land")



        # if(self.vehicle.mode.name == )
    def command_run(self,command):
        if(self.vehicle.mode.name == "GUIDED" and self.offboard_safety == False):
            if(command == "takeoff" and self.is_takeoff == False):
                self.takeoff()
            if(self.is_takeoff_running):
                self.takeoff()
            elif(self.is_land_running):
                self.land()

            elif(self.is_takeoff == True):
                if(command == "land"):
                    self.land()
                # if(command == "forward"):
                #     self.x_vel_set=0.15
                #     self.y_vel_set=0
                # if(command == "backward"):
                #     self.x_vel_set=-0.15
                #     self.y_vel_set=0
                if(command == "hover"):
                    self.x_vel_set=0
                    self.y_vel_set=0
                    
        elif(self.vehicle.mode.name == "GUIDED" and self.offboard_safety == True):
            print("[ERROR] please close offboard_safety mode")
            # else:
            #     print("[ERROR] The drone is not flying or is taking off")

              

            
        
    
